/**
 * @file task_gnssmgr.c
 * @brief
 * @author  ()
 * @version 1.0
 * @date 2024-09-13
 *
 * @copyright Copyright (C) 2019 Queclink Wireless Solutions (ShenZhen) Co., Ltd. All Rights Reserved.
 *
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version <th>Author  <th>Description
 * <tr><td>2024-09-13 <td>1.0     <td>     <td>内容
 * </table>
 */
#include <math.h>
#include <stdio.h>
#include "gnss_log.h"
#include <public/errno.h>
#include <task_gnss.h>
#include <qlcommon.h>
#include "bsp_gnss.h"
#include "misc/rtc_app.h"
#include "gnss_public.h"
#include "comm/co_heartbeat.h"
#include "task/misc/misc_system.h"
#include "gnss_request.h"
#include "gnss_power_mgr.h"
#include "lowpower/include/low_power.h"

/* 宏定义----------------------------------------------------------------- */


/* 结构体及函数声明----------------------------------------------------------------- */
extern bool mtkseries_ready;

/* gnss管理任务控制参数 */
typedef struct {
    int             stage;
    unsigned int    uptime;
} gnssmgr_cps_t;

typedef struct {
    bool            pwron;                          /* 是否处于上电状态 */
    bool            first_open;                     /* 是否设备上电首次开启 */
    uint32_t        open_tick;                      /* 最近一次开启的时间, unit:s */
    uint32_t        close_tick;                     /* 最近一次关闭的时间, unit:s */
    gnssmgr_cps_t   cps;                            /* 控制参数 */
} gnss_context_t;

/* 全局变量声明----------------------------------------------------------------- */
static SemaphoreHandle_t    gnssmgr_mutex;          /* 信号量保护 */
static struct fix_info      last_info;              /* 最近一次有效定位信息 */
static struct sat_info      last_sat;               /* 保存最后一次定位信息对应的卫星信息 */
static gnss_context_t       gnss_context;           /* 任务全局管理 */
static unsigned char        gnss_need_always_open;  /* GNSS 常开源 */


/* 函数实现----------------------------------------------------------------- */
void gnssmgrSemaphoreGive(void)
{
    xSemaphoreGive(gnssmgr_mutex);
}

int gnssmgrSemaphoreTake(void)
{
    if (NULL == gnssmgr_mutex) {
        taskENTER_CRITICAL();
        if (NULL == gnssmgr_mutex)
            gnssmgr_mutex = xSemaphoreCreateMutex();
        taskEXIT_CRITICAL();
    }
    if (NULL == gnssmgr_mutex)
        return -ENOMEM;
    if (pdFALSE == xSemaphoreTake(gnssmgr_mutex, portMAX_DELAY))
        return -EBUSY;
    return 0;
}

/**
 * @brief GNSS 全局参数初始化
 */
void gnssmgr_power_manage_init(void)
{
    memset(&gnss_context, 0, sizeof(gnss_context_t));
    gnss_context.first_open = true;
}

/**
 * @brief GNSS 上电状态获取
 */
bool gnss_power_status_get(void)
{
    return gnss_context.pwron;
}

/**
 * @brief GNSS 应用层开启操作
 * @return 0 - 成功, 负数 - 错误码
 */
static int gnss_open(void)
{
    bsp_gnss_power_on();
    gnss_context.pwron = true;
    gnss_context.open_tick = getsec();
    gnss_context.close_tick = 0;
    gnss_trace(LOG_INFO "GNSS open!\r\n");
    return 0;
}

/**
 * @brief GNSS 应用层关闭操作(后续如果需要按需开, 则补全逻辑)
 * @return 0 - 成功, 负数 - 错误码
 */
static void gnss_close(void)
{
    bsp_gnss_power_off();
    gnss_context.pwron = false;
    gnss_context.close_tick = getsec();
    gnss_trace(LOG_INFO "GNSS close!\r\n");
}

/**
 * @brief GNSS 常开状态获取
 * @return true - 常开, false - 按需开
 */
bool gnss_need_always_open_status(void)
{
    bool enable = false;

    gnssmgrSemaphoreTake();
    enable = gnss_need_always_open ? true : false;
    gnssmgrSemaphoreGive();

    return enable;
}

/**
 * @brief GNSS 常开状态获取
 * @return true - 常开, false - 按需开
 */
void set_gnss_need_always_open_mask(unsigned char code)
{
    gnssmgrSemaphoreTake();
    gnss_need_always_open |= code;
    gnssmgrSemaphoreGive();
}

/**
 * @brief GNSS 常开状态设置
 * @return true - 常开, false - 按需开
 */
void clr_gnss_need_always_open_mask(unsigned char code)
{
    gnssmgrSemaphoreTake();
    gnss_need_always_open &= ~code;
    gnssmgrSemaphoreGive();
}

/**
 * @brief 更新上一次的定位信息
 * @param gnss_info 定位信息
 */
void update_last_gnss_info(struct fix_info *gnss_info)
{
    gnssmgrSemaphoreTake();
    if (gnss_info != NULL)
        last_info       = *gnss_info;
    else
        last_info.state = FIX_INVALID;
    gnssmgrSemaphoreGive();
}

/**
 * @brief 暂时保持 GNSS 常开
 */
void gnss_always_open_handler(void)
{
    if (WORK_MODE_SLEEP != misc_get_work_mode())
        set_gnss_need_always_open_mask(ECU_ALWAYS_OPEN_GNSS);
    else
        clr_gnss_need_always_open_mask(ECU_ALWAYS_OPEN_GNSS);
}

typedef struct {
    float           min_heading;  /* 最小航向 */
    float           max_heading;  /* 最大航向 */
    struct fix_info base_gps;
} corner_info_t;

static corner_info_t corner_info = {
    .min_heading    = 0.0,
    .max_heading    = 0.0,
    .base_gps.state = 0,
};

/**
 * @brief 角度计算
 */
static float gps_corner_calculate(float head_heading, float tail_heading)
{
    float heading = fabs(head_heading - tail_heading);

    heading = heading > 180 ? fabs(360 - heading) : heading;

    return heading;
}

/**
 * @brief 判断角度变化
 * @param gps 上一次定位信息
 * @param update_base_point 是否更新基准点
 * @return true - 产生拐点
 */
bool check_corner(struct fix_info *gps, bool update_base_point)
{
    bool ret = false;
    float min_delta    = 0.0;
    float max_delta    = 0.0;
    float base_delta   = 0.0;
    float temp_delta   = 0.0;
    float curr_heading = 0.0;

    if (is_ebike_riding()) /* 非骑行模式不检测角度 */
        return ret;

    gnss_trace(LOG_INFO "gps corner fix=%d,speed=%.2f,heading=%.2f,%.2f,%.2f,%.2f,seconds:%d.\r\n",
              gps->state, gps->speed, gps->angle, corner_info.base_gps.angle,
              corner_info.min_heading, corner_info.max_heading, gps->utc.tm_sec);

    curr_heading = gps->angle;

    if (true == update_base_point) {           /* 定时或定距离更新基础点 */
        update_base_point = false;
        corner_info.base_gps    = *gps;
        corner_info.min_heading = curr_heading;
        corner_info.max_heading = curr_heading;
        //update_last_corner(gps, false);
        gnss_trace(LOG_INFO "Last Corner Update: Base\r\n");
        return ret;
    }

    if (FIX_OK == corner_info.base_gps.state) {
        base_delta = gps_corner_calculate(curr_heading, corner_info.base_gps.angle);
        min_delta  = gps_corner_calculate(curr_heading, corner_info.min_heading);
        max_delta  = gps_corner_calculate(curr_heading, corner_info.max_heading);

        log_i("min_delta = %.2f, max_delta = %.2f, base_delta = %.2f\r\n",
              min_delta,
              max_delta,
              base_delta);
        if ((min_delta  >= 45.0f) ||
            (max_delta  >= 45.0f) ||
            (base_delta >= 45.0f)) {
            corner_info.base_gps    = *gps;
            corner_info.min_heading = curr_heading;
            corner_info.max_heading = curr_heading;

            /* 拐点更新最后位置点，并触发GPS上报 */
            update_last_gnss_info(gps);
            corner_report_trigger();
            gnss_trace(LOG_INFO "Last Corner Update: Corner\r\n");
            ret = true;
        } else {
            /* 获取最大最小航向值 */
            temp_delta = corner_info.base_gps.angle - curr_heading;
            min_delta  = gps_corner_calculate(corner_info.base_gps.angle, corner_info.min_heading);
            max_delta  = gps_corner_calculate(corner_info.base_gps.angle, corner_info.max_heading);
            if (temp_delta > 0) {
                if (base_delta > min_delta)
                    corner_info.min_heading = curr_heading;
            } else {
                if (fabs(temp_delta) < 180) {
                    if (base_delta > max_delta)
                        corner_info.max_heading = curr_heading;
                } else {
                    if (base_delta > min_delta)
                        corner_info.min_heading = curr_heading;
                }
            }
        }
    } else {
        corner_info.base_gps    = *gps;
        corner_info.min_heading = curr_heading;
        corner_info.max_heading = curr_heading;
        //update_last_corner(gps, false);
        gnss_trace(LOG_INFO "Last Corner Update: Base\r\n");
    }

    return ret;
}

/**
 * @brief 更新当前的经纬度信息
 * @param new_ 当前定位信息
 * @param stderr_ 标准差
 */
void qltask_gnss_fix_get_hook(const struct fix_info *new_, double stderr_)
{
    struct fix_info *info = NULL;

    if (FIX_OK == new_->state) {
        /* 后续可能要修改定位信息 */
        if (NULL == (info = qlmalloc(sizeof(struct fix_info))))
            return;
        memcpy(info, new_, sizeof(struct fix_info));

        /* 标准差大于设置的阈值，则使用上一次的定位点 */
        if (GNSS_STRERR_THOLD < stderr_) {
            gnssmgrSemaphoreTake();
            info->latitude  = last_info.latitude;
            info->longitude = last_info.longitude;
            gnssmgrSemaphoreGive();
            update_last_gnss_info(info);
            gnss_trace(LOG_INFO"curr stderr %.1lf more than %d, used last gps\r\n", stderr_, GNSS_STRERR_THOLD);
            goto out;
        }
        update_last_gnss_info(info);
        check_corner(info, 0);
    }
    else if (last_info.state == FIX_OK) {
        update_last_gnss_info(NULL);
    }

out:
    if (NULL != info)
        qlfree(info);
    qltask_gnss_sat_get(&last_sat);
    fix_info_printf("Last fix information.    ", &last_info, last_sat.count, stderr_);
}

/**
 * @brief 获取上一次的经纬度信息
 * @param gnss_info 存放定位信息
 */
void get_last_gnss_info(struct fix_info *gnss_info)
{
    gnssmgrSemaphoreTake();
    if (gnss_info)
        *gnss_info = last_info;
    gnssmgrSemaphoreGive();
}

/**
 * @brief 获取上一次的卫星信息
 * @param sat 存放卫星信息
 * @param count 存放卫星颗数
 */
void get_last_sat_info(struct sat_info *sat, unsigned int *count)
{
    gnssmgrSemaphoreTake();
    if (count)
        *count = last_sat.count;
    if (sat)
        *sat = last_sat;
    gnssmgrSemaphoreGive();
}

/**
 * @brief 判断当前是否定位, 如果 gnss_info != NULL, 则同时返回当前的定位信息
 * @param gnss_info 存放定位信息
 */
bool is_gnss_fixed(struct fix_info *gnss_info)
{
    bool fix;

    gnssmgrSemaphoreTake();

    if (NULL != gnss_info)
        *gnss_info = last_info;

    fix = last_info.state != FIX_INVALID;
    gnssmgrSemaphoreGive();

    return fix;
}

/*
 * @brief 管理 GNSS 电源开关
 */
void gnssmgr_power_manage_process(void)
{
    static uint32_t log_timeout = 0;
    uint8_t         mask        = 0;
    bool            always_open = gnss_need_always_open_status();

    /*判断 GNSS 是否处于常开状态 */
    gnss_always_open_handler();

    /* GNSS 关闭3s后, 设置上一次定位状态为未定位 */
    if (0 != gnss_context.close_tick && timeout_sec(gnss_context.close_tick, 3)) {
        if (FIX_OK == is_gnss_fixed(NULL)) {
            update_last_gnss_info(NULL);
            gnss_trace(LOG_INFO "gnss close 3s, set last fix state invalid\r\n");
        }
    }

    switch (gnss_context.cps.stage) {
    case 0:                                             /* 等待开启GNSS */
        if (gnss_context.pwron) {
            if (gnss_context.first_open)                /* GNSS已开启则不再执行首次开启 */
                gnss_context.first_open = false;

            gnss_context.cps.stage  = 1;                /* 已开启，等待关闭 */
            gnss_context.cps.uptime = getms();
        }
        else {
            if (gnss_context.first_open)
                mask = 1;
            else if (always_open)
                mask = 2;
            else if (!gnssmgr_request_is_empty())
                mask = 3;

            if (0 != mask) {
                gnss_open();
                gnss_trace(LOG_INFO "current gnss open and mask is %d\r\n", mask);
            }
        }
        break;

    case 1: /* 等待关闭GNSS(目前GNSS保持常开, 后续按需开需要调整) */
        if (!gnss_context.pwron) {
            gnss_context.cps.stage = 0;
        }
        else {
            if (timeout_sec(gnss_context.open_tick, GNSS_FIX_TIMEOUT_DEFAULT) || is_gnss_fixed(NULL)) /* 超时定位或定位成功 */
                mask |= GNSS_CLOSE_MASK_FIX_END;
            if (!always_open) /* GNSS常开 */
                mask |= GNSS_CLOSE_MASK_NONEED_ALWAYS;
            if (gnssmgr_request_is_empty())
                mask |= GNSS_CLOSE_MASK_REQUEST_IDLE;
            if (mtkseries_ready)
                mask |= GNSS_CLOSE_MASK_OPTION_END;

            if (GNSS_CAN_CLOSE_MASK == (mask & GNSS_CAN_CLOSE_MASK)) {
                gnss_close();
            }
            else {
                /* 定时打印 */
                if (timeout_ms(log_timeout, 5000)) {
                    log_timeout = getms();
                    gnss_trace(LOG_INFO "current gnss still needs open, check code[%02X], gnss_need_always_open=%02X\r\n",
                               mask, gnss_need_always_open);
                }
            }
        }
        break;

    default:
        break;
    }

}

/**
 * @brief GNSS任务休眠时间
 * @return
 */
unsigned int qltask_gnss_canstop(void)
{
    if (!gnss_context.pwron)
        return TASK_ALWAYS_SLEEP;

    return 0;
}

/**
 * @brief GPS任务中在GPS时间秒数跳变时，同步RTC时间
 */
void reflesh_rtc_time_from_gps(void)
{
    static uint32_t     old_ms  = 0;
    static struct tm    old_utc = {0};

    if (timeout_ms(old_ms, 100)) {
        struct fix_info gps_cur_info;
        old_ms = getms();
	    if (is_gnss_fixed(&gps_cur_info) && memcmp((void *)&old_utc, (void *)&gps_cur_info.utc, sizeof(struct tm))) {
            rtc_update_handler();
            memcpy(&old_utc, &gps_cur_info.utc, sizeof(struct tm));
        }
    }
}

